#include "stm32f10x.h"                  // Device header
#include "Motor.h"
#include "IR.h"
#include "Bluetooth.h"
#include "HCSR04.h"
#include "InfrTrackSensor.h"

uint8_t Infr_Flag;

/**
  * @brief  红外传感器初始化
  * @param  无
  * @retval 无
  */
void InfrTrackSensor_Init()
{
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_GPIOA,ENABLE);
	RCC_APB2PeriphClockCmd(RCC_APB2Periph_AFIO, ENABLE);
	
	GPIO_InitTypeDef GPIO_InitStructure;
	GPIO_InitStructure.GPIO_Mode=GPIO_Mode_IPD;
	GPIO_InitStructure.GPIO_Pin=GPIO_Pin_4|GPIO_Pin_5|GPIO_Pin_6|GPIO_Pin_7;
	GPIO_InitStructure.GPIO_Speed=GPIO_Speed_50MHz;
	GPIO_Init(GPIOA,&GPIO_InitStructure);
	
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource4);  //使能4个引脚的中断
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource5);
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource6);
	GPIO_EXTILineConfig(GPIO_PortSourceGPIOA,GPIO_PinSource7);
	
	EXTI_InitTypeDef EXTI_InitStructure;
	EXTI_InitStructure.EXTI_Line=EXTI_Line4|EXTI_Line5|EXTI_Line6|EXTI_Line7;
	EXTI_InitStructure.EXTI_LineCmd=ENABLE;
	EXTI_InitStructure.EXTI_Mode=EXTI_Mode_Interrupt;
	EXTI_InitStructure.EXTI_Trigger=EXTI_Trigger_Rising_Falling;
	EXTI_Init(&EXTI_InitStructure);
	
	NVIC_PriorityGroupConfig(NVIC_PriorityGroup_2);
	NVIC_InitTypeDef NVIC_InitStructure;
	NVIC_InitStructure.NVIC_IRQChannel=EXTI9_5_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=1;
	NVIC_Init(&NVIC_InitStructure);
	
	NVIC_InitStructure.NVIC_IRQChannel=EXTI4_IRQn;
	NVIC_InitStructure.NVIC_IRQChannelCmd=ENABLE;
	NVIC_InitStructure.NVIC_IRQChannelPreemptionPriority=1;
	NVIC_InitStructure.NVIC_IRQChannelSubPriority=2;
	NVIC_Init(&NVIC_InitStructure);
}

void Infr_AdjustCar(SetMotor_Speed* Infr_Speed)
{
	if((GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==1||GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6)==1)&&GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_4) == 0 && GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 0)
	{
		//小车前行
		Infr_Speed->Speed1=80;
		Infr_Speed->Speed2=80;
		Infr_Speed->Speed3=80;
		Infr_Speed->Speed4=80;
	}
	else if((GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==0&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==0 && GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 1))
	{
		// 小车偏右
		Infr_Speed->Speed1=-100;
		Infr_Speed->Speed2=100;
		Infr_Speed->Speed3=-100;
		Infr_Speed->Speed4=100;			
	}	
	else if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==0&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==1&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6)==1&&GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 1)
	{
		// 小车偏右
		Infr_Speed->Speed1=-100;
		Infr_Speed->Speed2=100;
		Infr_Speed->Speed3=-100;
		Infr_Speed->Speed4=100;			
	}		
	else if((GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==1&&GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_6) == 0 && GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 0))
	{
		//小车偏左
		Infr_Speed->Speed1=100;
		Infr_Speed->Speed2=-100;
		Infr_Speed->Speed3=100;
		Infr_Speed->Speed4=-100;			
	}
	else if(GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_4)==1&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_5)==1&&GPIO_ReadInputDataBit(GPIOA,GPIO_Pin_6)==1&&GPIO_ReadInputDataBit(GPIOA, GPIO_Pin_7) == 0)
	{
		// 小车偏左
		Infr_Speed->Speed1=-100;
		Infr_Speed->Speed2=100;
		Infr_Speed->Speed3=-100;
		Infr_Speed->Speed4=100;			
	}	
}

/**
  * @brief  获取标志位
  * @param  无
  * @retval 返回标志位
  */
uint8_t Infr_GetFlag()
{
	return Infr_Flag;
}

/**
  * @brief  设置标志位
  * @param  Value：设置一个Infr_SetValue枚举类型的值，该值可以为：INFR_RESET，INFR_SET
  * @retval 无
  */
void Infr_SetFlag(Infr_SetValue Value)
{
	Infr_Flag=Value;
}
